#include "aim_tower/HitTower.hpp"

void HitTower::reset(){
    init_time = std::chrono::system_clock::now().time_since_epoch().count() / 1e9;
    history_x.clear();
    history_z.clear();
}

double HitTower::getCurrentTime(){ 
    return (std::chrono::system_clock::now().time_since_epoch().count() / 1e9) - init_time;
} 

void HitTower::initWithROS(ros::NodeHandle& nh) {
    air_k = readParam<double>(nh, "/gimbal_k_value");
}


double calPredicTime(float dist,float bullet_v, float pitch,float air_k){
    /*
        视觉识别的时间(8ms左右)
        通讯时间(0.5ms左右)
        电控处理时间
        控制时间
        子弹滞空时间(关键影响)
    */
    float buller_time = (-1 / (air_k)*logf(1 - air_k * dist / (bullet_v * cosf(pitch))));
    return 0.01 + buller_time;
}
#define _RAD2DEG 57.29578
void HitTower::hit(const cv::Point3f& t_cam,const serial_com::comm& msg, Eigen::Vector3d& cam_p){
    //从相机坐标系转到世界坐标系
    Eigen::Vector3d pw;//世界坐标系下的坐标
    Eigen::Quaterniond c2w;
    project2World(t_cam, msg.y, msg.x, pw, c2w); 

    history_x.push_front(pw(0));
    history_z.push_front(pw(2));
    
    double current_time = getCurrentTime();
    history_t.push_front(current_time);

    if(history_t.size() > 200){
        history_x.pop_back();
        history_z.pop_back();
        history_t.pop_back();
        //计算预测时间
        float dist = std::sqrt(t_cam.x * t_cam.x + t_cam.z * t_cam.z);
        float predict_time = calPredicTime(dist, msg.z, msg.y / _RAD2DEG, air_k);
        //寻找上一个周期装甲板的位置
        for(int i = 0; i < history_t.size(); i++){
            if(std::fabs(current_time - history_t[i] - (spin_period/3.0 - predict_time)) < time_eps){
                Eigen::Vector3d result(history_x[i],pw(1),history_z[i]); 
                cam_p = c2w.conjugate() * result;
            }

        }
    }else{
        return;
    }


}